{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# A Discrete Switching System\n",
    "\n",
    "A la multi-hypothesis-smoother (MHS), but all discrete.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "tags": [
     "remove-cell"
    ]
   },
   "source": [
    "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n",
    "Atlanta, Georgia 30332-0415\n",
    "All Rights Reserved\n",
    "\n",
    "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n",
    "\n",
    "See LICENSE for the license information"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/gtsam/examples/DiscreteSwitching.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [],
   "source": [
    "# This needs gtbook:\n",
    "% pip install --quiet gtbook"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from gtsam import DiscreteBayesNet, DiscreteKeys, DiscreteFactorGraph, Ordering\n",
    "from gtsam.symbol_shorthand import S\n",
    "from gtsam.symbol_shorthand import M"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def P(*args):\n",
    "    \"\"\" Create a DiscreteKeys instances from a variable number of DiscreteKey pairs.\"\"\"\n",
    "    # TODO: We can make life easier by providing variable argument functions in C++ itself.\n",
    "    dks = DiscreteKeys()\n",
    "    for key in args:\n",
    "        dks.push_back(key)\n",
    "    return dks\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "import graphviz\n",
    "\n",
    "\n",
    "class show(graphviz.Source):\n",
    "    \"\"\" Display an object with a dot method as a graph.\"\"\"\n",
    "\n",
    "    def __init__(self, obj):\n",
    "        \"\"\"Construct from object with 'dot' method.\"\"\"\n",
    "        # This small class takes an object, calls its dot function, and uses the\n",
    "        # resulting string to initialize a graphviz.Source instance. This in turn\n",
    "        # has a _repr_mimebundle_ method, which then renders it in the notebook.\n",
    "        super().__init__(obj.dot())\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<div><p><tt>DiscreteBayesNet</tt> of size 4</p><div>\n",
       "<p>  <i>P(s2|m1,s1):</i></p>\n",
       "<table class='DiscreteConditional'>\n",
       "  <thead>\n",
       "    <tr><th><i>m1</i></th><th><i>s1</i></th><th>0</th><th>1</th><th>2</th></tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr><th>0</th><th>0</th><td>0.9</td><td>0.1</td><td>0</td></tr>\n",
       "    <tr><th>0</th><th>1</th><td>0.1</td><td>0.8</td><td>0.1</td></tr>\n",
       "    <tr><th>0</th><th>2</th><td>0</td><td>0.1</td><td>0.9</td></tr>\n",
       "    <tr><th>1</th><th>0</th><td>0.1</td><td>0.9</td><td>0</td></tr>\n",
       "    <tr><th>1</th><th>1</th><td>0</td><td>0.1</td><td>0.9</td></tr>\n",
       "    <tr><th>1</th><th>2</th><td>0.9</td><td>0</td><td>0.1</td></tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "</div>\n",
       "<div>\n",
       "<p>  <i>P(s3|m2,s2):</i></p>\n",
       "<table class='DiscreteConditional'>\n",
       "  <thead>\n",
       "    <tr><th><i>m2</i></th><th><i>s2</i></th><th>0</th><th>1</th><th>2</th></tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr><th>0</th><th>0</th><td>0.9</td><td>0.1</td><td>0</td></tr>\n",
       "    <tr><th>0</th><th>1</th><td>0.1</td><td>0.8</td><td>0.1</td></tr>\n",
       "    <tr><th>0</th><th>2</th><td>0</td><td>0.1</td><td>0.9</td></tr>\n",
       "    <tr><th>1</th><th>0</th><td>0.1</td><td>0.9</td><td>0</td></tr>\n",
       "    <tr><th>1</th><th>1</th><td>0</td><td>0.1</td><td>0.9</td></tr>\n",
       "    <tr><th>1</th><th>2</th><td>0.9</td><td>0</td><td>0.1</td></tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "</div>\n",
       "<div>\n",
       "<p>  <i>P(s4|m3,s3):</i></p>\n",
       "<table class='DiscreteConditional'>\n",
       "  <thead>\n",
       "    <tr><th><i>m3</i></th><th><i>s3</i></th><th>0</th><th>1</th><th>2</th></tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr><th>0</th><th>0</th><td>0.9</td><td>0.1</td><td>0</td></tr>\n",
       "    <tr><th>0</th><th>1</th><td>0.1</td><td>0.8</td><td>0.1</td></tr>\n",
       "    <tr><th>0</th><th>2</th><td>0</td><td>0.1</td><td>0.9</td></tr>\n",
       "    <tr><th>1</th><th>0</th><td>0.1</td><td>0.9</td><td>0</td></tr>\n",
       "    <tr><th>1</th><th>1</th><td>0</td><td>0.1</td><td>0.9</td></tr>\n",
       "    <tr><th>1</th><th>2</th><td>0.9</td><td>0</td><td>0.1</td></tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "</div>\n",
       "<div>\n",
       "<p>  <i>P(s5|m4,s4):</i></p>\n",
       "<table class='DiscreteConditional'>\n",
       "  <thead>\n",
       "    <tr><th><i>m4</i></th><th><i>s4</i></th><th>0</th><th>1</th><th>2</th></tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr><th>0</th><th>0</th><td>0.9</td><td>0.1</td><td>0</td></tr>\n",
       "    <tr><th>0</th><th>1</th><td>0.1</td><td>0.8</td><td>0.1</td></tr>\n",
       "    <tr><th>0</th><th>2</th><td>0</td><td>0.1</td><td>0.9</td></tr>\n",
       "    <tr><th>1</th><th>0</th><td>0.1</td><td>0.9</td><td>0</td></tr>\n",
       "    <tr><th>1</th><th>1</th><td>0</td><td>0.1</td><td>0.9</td></tr>\n",
       "    <tr><th>1</th><th>2</th><td>0.9</td><td>0</td><td>0.1</td></tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "</div>\n"
      ],
      "text/markdown": [
       "`DiscreteBayesNet` of size 4\n",
       "\n",
       " *P(s2|m1,s1):*\n",
       "\n",
       "|*m1*|*s1*|0|1|2|\n",
       "|:-:|:-:|:-:|:-:|:-:|\n",
       "|0|0|0.9|0.1|0|\n",
       "|0|1|0.1|0.8|0.1|\n",
       "|0|2|0|0.1|0.9|\n",
       "|1|0|0.1|0.9|0|\n",
       "|1|1|0|0.1|0.9|\n",
       "|1|2|0.9|0|0.1|\n",
       "\n",
       " *P(s3|m2,s2):*\n",
       "\n",
       "|*m2*|*s2*|0|1|2|\n",
       "|:-:|:-:|:-:|:-:|:-:|\n",
       "|0|0|0.9|0.1|0|\n",
       "|0|1|0.1|0.8|0.1|\n",
       "|0|2|0|0.1|0.9|\n",
       "|1|0|0.1|0.9|0|\n",
       "|1|1|0|0.1|0.9|\n",
       "|1|2|0.9|0|0.1|\n",
       "\n",
       " *P(s4|m3,s3):*\n",
       "\n",
       "|*m3*|*s3*|0|1|2|\n",
       "|:-:|:-:|:-:|:-:|:-:|\n",
       "|0|0|0.9|0.1|0|\n",
       "|0|1|0.1|0.8|0.1|\n",
       "|0|2|0|0.1|0.9|\n",
       "|1|0|0.1|0.9|0|\n",
       "|1|1|0|0.1|0.9|\n",
       "|1|2|0.9|0|0.1|\n",
       "\n",
       " *P(s5|m4,s4):*\n",
       "\n",
       "|*m4*|*s4*|0|1|2|\n",
       "|:-:|:-:|:-:|:-:|:-:|\n",
       "|0|0|0.9|0.1|0|\n",
       "|0|1|0.1|0.8|0.1|\n",
       "|0|2|0|0.1|0.9|\n",
       "|1|0|0.1|0.9|0|\n",
       "|1|1|0|0.1|0.9|\n",
       "|1|2|0.9|0|0.1|\n",
       "\n"
      ],
      "text/plain": [
       "DiscreteBayesNet\n",
       " \n",
       "size: 4\n",
       "conditional 0:  P( s2 | m1 s1 ):\n",
       " Choice(s2) \n",
       " 0 Choice(s1) \n",
       " 0 0 Choice(m1) \n",
       " 0 0 0 Leaf  0.9\n",
       " 0 0 1 Leaf  0.1\n",
       " 0 1 Choice(m1) \n",
       " 0 1 0 Leaf  0.1\n",
       " 0 1 1 Leaf    0\n",
       " 0 2 Choice(m1) \n",
       " 0 2 0 Leaf    0\n",
       " 0 2 1 Leaf  0.9\n",
       " 1 Choice(s1) \n",
       " 1 0 Choice(m1) \n",
       " 1 0 0 Leaf  0.1\n",
       " 1 0 1 Leaf  0.9\n",
       " 1 1 Choice(m1) \n",
       " 1 1 0 Leaf  0.8\n",
       " 1 1 1 Leaf  0.1\n",
       " 1 2 Choice(m1) \n",
       " 1 2 0 Leaf  0.1\n",
       " 1 2 1 Leaf    0\n",
       " 2 Choice(s1) \n",
       " 2 0 Choice(m1) \n",
       " 2 0 0 Leaf    0\n",
       " 2 0 1 Leaf    0\n",
       " 2 1 Choice(m1) \n",
       " 2 1 0 Leaf  0.1\n",
       " 2 1 1 Leaf  0.9\n",
       " 2 2 Choice(m1) \n",
       " 2 2 0 Leaf  0.9\n",
       " 2 2 1 Leaf  0.1\n",
       "\n",
       "conditional 1:  P( s3 | m2 s2 ):\n",
       " Choice(s3) \n",
       " 0 Choice(s2) \n",
       " 0 0 Choice(m2) \n",
       " 0 0 0 Leaf  0.9\n",
       " 0 0 1 Leaf  0.1\n",
       " 0 1 Choice(m2) \n",
       " 0 1 0 Leaf  0.1\n",
       " 0 1 1 Leaf    0\n",
       " 0 2 Choice(m2) \n",
       " 0 2 0 Leaf    0\n",
       " 0 2 1 Leaf  0.9\n",
       " 1 Choice(s2) \n",
       " 1 0 Choice(m2) \n",
       " 1 0 0 Leaf  0.1\n",
       " 1 0 1 Leaf  0.9\n",
       " 1 1 Choice(m2) \n",
       " 1 1 0 Leaf  0.8\n",
       " 1 1 1 Leaf  0.1\n",
       " 1 2 Choice(m2) \n",
       " 1 2 0 Leaf  0.1\n",
       " 1 2 1 Leaf    0\n",
       " 2 Choice(s2) \n",
       " 2 0 Choice(m2) \n",
       " 2 0 0 Leaf    0\n",
       " 2 0 1 Leaf    0\n",
       " 2 1 Choice(m2) \n",
       " 2 1 0 Leaf  0.1\n",
       " 2 1 1 Leaf  0.9\n",
       " 2 2 Choice(m2) \n",
       " 2 2 0 Leaf  0.9\n",
       " 2 2 1 Leaf  0.1\n",
       "\n",
       "conditional 2:  P( s4 | m3 s3 ):\n",
       " Choice(s4) \n",
       " 0 Choice(s3) \n",
       " 0 0 Choice(m3) \n",
       " 0 0 0 Leaf  0.9\n",
       " 0 0 1 Leaf  0.1\n",
       " 0 1 Choice(m3) \n",
       " 0 1 0 Leaf  0.1\n",
       " 0 1 1 Leaf    0\n",
       " 0 2 Choice(m3) \n",
       " 0 2 0 Leaf    0\n",
       " 0 2 1 Leaf  0.9\n",
       " 1 Choice(s3) \n",
       " 1 0 Choice(m3) \n",
       " 1 0 0 Leaf  0.1\n",
       " 1 0 1 Leaf  0.9\n",
       " 1 1 Choice(m3) \n",
       " 1 1 0 Leaf  0.8\n",
       " 1 1 1 Leaf  0.1\n",
       " 1 2 Choice(m3) \n",
       " 1 2 0 Leaf  0.1\n",
       " 1 2 1 Leaf    0\n",
       " 2 Choice(s3) \n",
       " 2 0 Choice(m3) \n",
       " 2 0 0 Leaf    0\n",
       " 2 0 1 Leaf    0\n",
       " 2 1 Choice(m3) \n",
       " 2 1 0 Leaf  0.1\n",
       " 2 1 1 Leaf  0.9\n",
       " 2 2 Choice(m3) \n",
       " 2 2 0 Leaf  0.9\n",
       " 2 2 1 Leaf  0.1\n",
       "\n",
       "conditional 3:  P( s5 | m4 s4 ):\n",
       " Choice(s5) \n",
       " 0 Choice(s4) \n",
       " 0 0 Choice(m4) \n",
       " 0 0 0 Leaf  0.9\n",
       " 0 0 1 Leaf  0.1\n",
       " 0 1 Choice(m4) \n",
       " 0 1 0 Leaf  0.1\n",
       " 0 1 1 Leaf    0\n",
       " 0 2 Choice(m4) \n",
       " 0 2 0 Leaf    0\n",
       " 0 2 1 Leaf  0.9\n",
       " 1 Choice(s4) \n",
       " 1 0 Choice(m4) \n",
       " 1 0 0 Leaf  0.1\n",
       " 1 0 1 Leaf  0.9\n",
       " 1 1 Choice(m4) \n",
       " 1 1 0 Leaf  0.8\n",
       " 1 1 1 Leaf  0.1\n",
       " 1 2 Choice(m4) \n",
       " 1 2 0 Leaf  0.1\n",
       " 1 2 1 Leaf    0\n",
       " 2 Choice(s4) \n",
       " 2 0 Choice(m4) \n",
       " 2 0 0 Leaf    0\n",
       " 2 0 1 Leaf    0\n",
       " 2 1 Choice(m4) \n",
       " 2 1 0 Leaf  0.1\n",
       " 2 1 1 Leaf  0.9\n",
       " 2 2 Choice(m4) \n",
       " 2 2 0 Leaf  0.9\n",
       " 2 2 1 Leaf  0.1\n"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "nrStates = 3\n",
    "K = 5\n",
    "\n",
    "bayesNet = DiscreteBayesNet()\n",
    "for k in range(1, K):\n",
    "    key = S(k), nrStates\n",
    "    key_plus = S(k+1), nrStates\n",
    "    mode = M(k), 2\n",
    "    bayesNet.add(key_plus, P(mode, key), \"9/1/0 1/8/1 0/1/9  1/9/0 0/1/9 9/0/1\")\n",
    "\n",
    "bayesNet"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/svg+xml": [
       "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
       "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
       " \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
       "<!-- Generated by graphviz version 12.0.0 (0)\n",
       " -->\n",
       "<!-- Pages: 1 -->\n",
       "<svg width=\"242pt\" height=\"332pt\"\n",
       " viewBox=\"0.00 0.00 242.00 332.00\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
       "<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 328)\">\n",
       "<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-328 238,-328 238,4 -4,4\"/>\n",
       "<!-- var7854277750134145025 -->\n",
       "<g id=\"node1\" class=\"node\">\n",
       "<title>var7854277750134145025</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"27\" cy=\"-306\" rx=\"27\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"27\" y=\"-300.95\" font-family=\"Times,serif\" font-size=\"14.00\">m1</text>\n",
       "</g>\n",
       "<!-- var8286623314361712642 -->\n",
       "<g id=\"node6\" class=\"node\">\n",
       "<title>var8286623314361712642</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"63\" cy=\"-234\" rx=\"27\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"63\" y=\"-228.95\" font-family=\"Times,serif\" font-size=\"14.00\">s2</text>\n",
       "</g>\n",
       "<!-- var7854277750134145025&#45;&gt;var8286623314361712642 -->\n",
       "<g id=\"edge7\" class=\"edge\">\n",
       "<title>var7854277750134145025&#45;&gt;var8286623314361712642</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M35.35,-288.76C39.58,-280.55 44.81,-270.37 49.58,-261.09\"/>\n",
       "<polygon fill=\"black\" stroke=\"black\" points=\"52.54,-262.99 54,-252.49 46.32,-259.79 52.54,-262.99\"/>\n",
       "</g>\n",
       "<!-- var7854277750134145026 -->\n",
       "<g id=\"node2\" class=\"node\">\n",
       "<title>var7854277750134145026</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"135\" cy=\"-234\" rx=\"27\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"135\" y=\"-228.95\" font-family=\"Times,serif\" font-size=\"14.00\">m2</text>\n",
       "</g>\n",
       "<!-- var8286623314361712643 -->\n",
       "<g id=\"node7\" class=\"node\">\n",
       "<title>var8286623314361712643</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-162\" rx=\"27\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"99\" y=\"-156.95\" font-family=\"Times,serif\" font-size=\"14.00\">s3</text>\n",
       "</g>\n",
       "<!-- var7854277750134145026&#45;&gt;var8286623314361712643 -->\n",
       "<g id=\"edge5\" class=\"edge\">\n",
       "<title>var7854277750134145026&#45;&gt;var8286623314361712643</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M126.65,-216.76C122.42,-208.55 117.19,-198.37 112.42,-189.09\"/>\n",
       "<polygon fill=\"black\" stroke=\"black\" points=\"115.68,-187.79 108,-180.49 109.46,-190.99 115.68,-187.79\"/>\n",
       "</g>\n",
       "<!-- var7854277750134145027 -->\n",
       "<g id=\"node3\" class=\"node\">\n",
       "<title>var7854277750134145027</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"171\" cy=\"-162\" rx=\"27\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"171\" y=\"-156.95\" font-family=\"Times,serif\" font-size=\"14.00\">m3</text>\n",
       "</g>\n",
       "<!-- var8286623314361712644 -->\n",
       "<g id=\"node8\" class=\"node\">\n",
       "<title>var8286623314361712644</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"135\" cy=\"-90\" rx=\"27\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"135\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">s4</text>\n",
       "</g>\n",
       "<!-- var7854277750134145027&#45;&gt;var8286623314361712644 -->\n",
       "<g id=\"edge3\" class=\"edge\">\n",
       "<title>var7854277750134145027&#45;&gt;var8286623314361712644</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M162.65,-144.76C158.42,-136.55 153.19,-126.37 148.42,-117.09\"/>\n",
       "<polygon fill=\"black\" stroke=\"black\" points=\"151.68,-115.79 144,-108.49 145.46,-118.99 151.68,-115.79\"/>\n",
       "</g>\n",
       "<!-- var7854277750134145028 -->\n",
       "<g id=\"node4\" class=\"node\">\n",
       "<title>var7854277750134145028</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"207\" cy=\"-90\" rx=\"27\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"207\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">m4</text>\n",
       "</g>\n",
       "<!-- var8286623314361712645 -->\n",
       "<g id=\"node9\" class=\"node\">\n",
       "<title>var8286623314361712645</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"171\" cy=\"-18\" rx=\"27\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"171\" y=\"-12.95\" font-family=\"Times,serif\" font-size=\"14.00\">s5</text>\n",
       "</g>\n",
       "<!-- var7854277750134145028&#45;&gt;var8286623314361712645 -->\n",
       "<g id=\"edge1\" class=\"edge\">\n",
       "<title>var7854277750134145028&#45;&gt;var8286623314361712645</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M198.65,-72.76C194.42,-64.55 189.19,-54.37 184.42,-45.09\"/>\n",
       "<polygon fill=\"black\" stroke=\"black\" points=\"187.68,-43.79 180,-36.49 181.46,-46.99 187.68,-43.79\"/>\n",
       "</g>\n",
       "<!-- var8286623314361712641 -->\n",
       "<g id=\"node5\" class=\"node\">\n",
       "<title>var8286623314361712641</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-306\" rx=\"27\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"99\" y=\"-300.95\" font-family=\"Times,serif\" font-size=\"14.00\">s1</text>\n",
       "</g>\n",
       "<!-- var8286623314361712641&#45;&gt;var8286623314361712642 -->\n",
       "<g id=\"edge8\" class=\"edge\">\n",
       "<title>var8286623314361712641&#45;&gt;var8286623314361712642</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M90.65,-288.76C86.42,-280.55 81.19,-270.37 76.42,-261.09\"/>\n",
       "<polygon fill=\"black\" stroke=\"black\" points=\"79.68,-259.79 72,-252.49 73.46,-262.99 79.68,-259.79\"/>\n",
       "</g>\n",
       "<!-- var8286623314361712642&#45;&gt;var8286623314361712643 -->\n",
       "<g id=\"edge6\" class=\"edge\">\n",
       "<title>var8286623314361712642&#45;&gt;var8286623314361712643</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M71.35,-216.76C75.58,-208.55 80.81,-198.37 85.58,-189.09\"/>\n",
       "<polygon fill=\"black\" stroke=\"black\" points=\"88.54,-190.99 90,-180.49 82.32,-187.79 88.54,-190.99\"/>\n",
       "</g>\n",
       "<!-- var8286623314361712643&#45;&gt;var8286623314361712644 -->\n",
       "<g id=\"edge4\" class=\"edge\">\n",
       "<title>var8286623314361712643&#45;&gt;var8286623314361712644</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M107.35,-144.76C111.58,-136.55 116.81,-126.37 121.58,-117.09\"/>\n",
       "<polygon fill=\"black\" stroke=\"black\" points=\"124.54,-118.99 126,-108.49 118.32,-115.79 124.54,-118.99\"/>\n",
       "</g>\n",
       "<!-- var8286623314361712644&#45;&gt;var8286623314361712645 -->\n",
       "<g id=\"edge2\" class=\"edge\">\n",
       "<title>var8286623314361712644&#45;&gt;var8286623314361712645</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M143.35,-72.76C147.58,-64.55 152.81,-54.37 157.58,-45.09\"/>\n",
       "<polygon fill=\"black\" stroke=\"black\" points=\"160.54,-46.99 162,-36.49 154.32,-43.79 160.54,-46.99\"/>\n",
       "</g>\n",
       "</g>\n",
       "</svg>\n"
      ],
      "text/plain": [
       "<__main__.show at 0x11216aea0>"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "show(bayesNet)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/svg+xml": [
       "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
       "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
       " \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
       "<!-- Generated by graphviz version 12.0.0 (0)\n",
       " -->\n",
       "<!-- Pages: 1 -->\n",
       "<svg width=\"360pt\" height=\"47pt\"\n",
       " viewBox=\"0.00 0.00 360.00 47.17\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
       "<g id=\"graph0\" class=\"graph\" transform=\"scale(0.564263 0.564263) rotate(0) translate(4 79.6)\">\n",
       "<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-79.6 634,-79.6 634,4 -4,4\"/>\n",
       "<!-- var7854277750134145025 -->\n",
       "<g id=\"node1\" class=\"node\">\n",
       "<title>var7854277750134145025</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"27\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"27\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">m1</text>\n",
       "</g>\n",
       "<!-- factor0 -->\n",
       "<g id=\"node10\" class=\"node\">\n",
       "<title>factor0</title>\n",
       "<ellipse fill=\"black\" stroke=\"black\" cx=\"99\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
       "</g>\n",
       "<!-- var7854277750134145025&#45;&#45;factor0 -->\n",
       "<g id=\"edge2\" class=\"edge\">\n",
       "<title>var7854277750134145025&#45;&#45;factor0</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M44.43,-43.58C63.74,-29.15 92.87,-7.38 98.16,-3.43\"/>\n",
       "</g>\n",
       "<!-- var7854277750134145026 -->\n",
       "<g id=\"node2\" class=\"node\">\n",
       "<title>var7854277750134145026</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"243\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"243\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">m2</text>\n",
       "</g>\n",
       "<!-- factor1 -->\n",
       "<g id=\"node11\" class=\"node\">\n",
       "<title>factor1</title>\n",
       "<ellipse fill=\"black\" stroke=\"black\" cx=\"243\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
       "</g>\n",
       "<!-- var7854277750134145026&#45;&#45;factor1 -->\n",
       "<g id=\"edge5\" class=\"edge\">\n",
       "<title>var7854277750134145026&#45;&#45;factor1</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M243,-39.28C243,-25.77 243,-8.54 243,-3.96\"/>\n",
       "</g>\n",
       "<!-- var7854277750134145027 -->\n",
       "<g id=\"node3\" class=\"node\">\n",
       "<title>var7854277750134145027</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"387\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"387\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">m3</text>\n",
       "</g>\n",
       "<!-- factor2 -->\n",
       "<g id=\"node12\" class=\"node\">\n",
       "<title>factor2</title>\n",
       "<ellipse fill=\"black\" stroke=\"black\" cx=\"387\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
       "</g>\n",
       "<!-- var7854277750134145027&#45;&#45;factor2 -->\n",
       "<g id=\"edge8\" class=\"edge\">\n",
       "<title>var7854277750134145027&#45;&#45;factor2</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M387,-39.28C387,-25.77 387,-8.54 387,-3.96\"/>\n",
       "</g>\n",
       "<!-- var7854277750134145028 -->\n",
       "<g id=\"node4\" class=\"node\">\n",
       "<title>var7854277750134145028</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"531\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"531\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">m4</text>\n",
       "</g>\n",
       "<!-- factor3 -->\n",
       "<g id=\"node13\" class=\"node\">\n",
       "<title>factor3</title>\n",
       "<ellipse fill=\"black\" stroke=\"black\" cx=\"531\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
       "</g>\n",
       "<!-- var7854277750134145028&#45;&#45;factor3 -->\n",
       "<g id=\"edge11\" class=\"edge\">\n",
       "<title>var7854277750134145028&#45;&#45;factor3</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M531,-39.28C531,-25.77 531,-8.54 531,-3.96\"/>\n",
       "</g>\n",
       "<!-- var8286623314361712641 -->\n",
       "<g id=\"node5\" class=\"node\">\n",
       "<title>var8286623314361712641</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"99\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">s1</text>\n",
       "</g>\n",
       "<!-- var8286623314361712641&#45;&#45;factor0 -->\n",
       "<g id=\"edge3\" class=\"edge\">\n",
       "<title>var8286623314361712641&#45;&#45;factor0</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M99,-39.28C99,-25.77 99,-8.54 99,-3.96\"/>\n",
       "</g>\n",
       "<!-- var8286623314361712642 -->\n",
       "<g id=\"node6\" class=\"node\">\n",
       "<title>var8286623314361712642</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"171\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"171\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">s2</text>\n",
       "</g>\n",
       "<!-- var8286623314361712642&#45;&#45;factor0 -->\n",
       "<g id=\"edge1\" class=\"edge\">\n",
       "<title>var8286623314361712642&#45;&#45;factor0</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M153.57,-43.58C134.26,-29.15 105.13,-7.38 99.84,-3.43\"/>\n",
       "</g>\n",
       "<!-- var8286623314361712642&#45;&#45;factor1 -->\n",
       "<g id=\"edge6\" class=\"edge\">\n",
       "<title>var8286623314361712642&#45;&#45;factor1</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M188.43,-43.58C207.74,-29.15 236.87,-7.38 242.16,-3.43\"/>\n",
       "</g>\n",
       "<!-- var8286623314361712643 -->\n",
       "<g id=\"node7\" class=\"node\">\n",
       "<title>var8286623314361712643</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"315\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"315\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">s3</text>\n",
       "</g>\n",
       "<!-- var8286623314361712643&#45;&#45;factor1 -->\n",
       "<g id=\"edge4\" class=\"edge\">\n",
       "<title>var8286623314361712643&#45;&#45;factor1</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M297.57,-43.58C278.26,-29.15 249.13,-7.38 243.84,-3.43\"/>\n",
       "</g>\n",
       "<!-- var8286623314361712643&#45;&#45;factor2 -->\n",
       "<g id=\"edge9\" class=\"edge\">\n",
       "<title>var8286623314361712643&#45;&#45;factor2</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M332.43,-43.58C351.74,-29.15 380.87,-7.38 386.16,-3.43\"/>\n",
       "</g>\n",
       "<!-- var8286623314361712644 -->\n",
       "<g id=\"node8\" class=\"node\">\n",
       "<title>var8286623314361712644</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"459\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"459\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">s4</text>\n",
       "</g>\n",
       "<!-- var8286623314361712644&#45;&#45;factor2 -->\n",
       "<g id=\"edge7\" class=\"edge\">\n",
       "<title>var8286623314361712644&#45;&#45;factor2</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M441.57,-43.58C422.26,-29.15 393.13,-7.38 387.84,-3.43\"/>\n",
       "</g>\n",
       "<!-- var8286623314361712644&#45;&#45;factor3 -->\n",
       "<g id=\"edge12\" class=\"edge\">\n",
       "<title>var8286623314361712644&#45;&#45;factor3</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M476.43,-43.58C495.74,-29.15 524.87,-7.38 530.16,-3.43\"/>\n",
       "</g>\n",
       "<!-- var8286623314361712645 -->\n",
       "<g id=\"node9\" class=\"node\">\n",
       "<title>var8286623314361712645</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"603\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"603\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">s5</text>\n",
       "</g>\n",
       "<!-- var8286623314361712645&#45;&#45;factor3 -->\n",
       "<g id=\"edge10\" class=\"edge\">\n",
       "<title>var8286623314361712645&#45;&#45;factor3</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M585.57,-43.58C566.26,-29.15 537.13,-7.38 531.84,-3.43\"/>\n",
       "</g>\n",
       "</g>\n",
       "</svg>\n"
      ],
      "text/plain": [
       "<__main__.show at 0x1121a44d0>"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Create a factor graph out of the Bayes net.\n",
    "factorGraph = DiscreteFactorGraph(bayesNet)\n",
    "show(factorGraph)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Position 0: s1, s2, s3, s4, s5, m1, m2, m3, m4\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# Create a BayesTree out of the factor graph.\n",
    "ordering = Ordering()\n",
    "# First eliminate \"continuous\" states in time order\n",
    "for k in range(1, K+1):\n",
    "    ordering.push_back(S(k))\n",
    "for k in range(1, K):\n",
    "    ordering.push_back(M(k))\n",
    "print(ordering)\n",
    "bayesTree = factorGraph.eliminateMultifrontal(ordering)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/svg+xml": [
       "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
       "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
       " \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
       "<!-- Generated by graphviz version 12.0.0 (0)\n",
       " -->\n",
       "<!-- Title: G Pages: 1 -->\n",
       "<svg width=\"212pt\" height=\"260pt\"\n",
       " viewBox=\"0.00 0.00 212.04 260.00\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
       "<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 256)\">\n",
       "<title>G</title>\n",
       "<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-256 208.04,-256 208.04,4 -4,4\"/>\n",
       "<!-- 0 -->\n",
       "<g id=\"node1\" class=\"node\">\n",
       "<title>0</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"102.02\" cy=\"-234\" rx=\"102.02\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"102.02\" y=\"-228.95\" font-family=\"Times,serif\" font-size=\"14.00\">s4, s5, m1, m2, m3, m4</text>\n",
       "</g>\n",
       "<!-- 1 -->\n",
       "<g id=\"node2\" class=\"node\">\n",
       "<title>1</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"102.02\" cy=\"-162\" rx=\"87.18\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"102.02\" y=\"-156.95\" font-family=\"Times,serif\" font-size=\"14.00\">s3 : m1, m2, m3, s4</text>\n",
       "</g>\n",
       "<!-- 0&#45;&gt;1 -->\n",
       "<g id=\"edge1\" class=\"edge\">\n",
       "<title>0&#45;&gt;1</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M102.02,-215.7C102.02,-208.41 102.02,-199.73 102.02,-191.54\"/>\n",
       "<polygon fill=\"black\" stroke=\"black\" points=\"105.52,-191.62 102.02,-181.62 98.52,-191.62 105.52,-191.62\"/>\n",
       "</g>\n",
       "<!-- 2 -->\n",
       "<g id=\"node3\" class=\"node\">\n",
       "<title>2</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"102.02\" cy=\"-90\" rx=\"69.78\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"102.02\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">s2 : m1, m2, s3</text>\n",
       "</g>\n",
       "<!-- 1&#45;&gt;2 -->\n",
       "<g id=\"edge2\" class=\"edge\">\n",
       "<title>1&#45;&gt;2</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M102.02,-143.7C102.02,-136.41 102.02,-127.73 102.02,-119.54\"/>\n",
       "<polygon fill=\"black\" stroke=\"black\" points=\"105.52,-119.62 102.02,-109.62 98.52,-119.62 105.52,-119.62\"/>\n",
       "</g>\n",
       "<!-- 3 -->\n",
       "<g id=\"node4\" class=\"node\">\n",
       "<title>3</title>\n",
       "<ellipse fill=\"none\" stroke=\"black\" cx=\"102.02\" cy=\"-18\" rx=\"52.38\" ry=\"18\"/>\n",
       "<text text-anchor=\"middle\" x=\"102.02\" y=\"-12.95\" font-family=\"Times,serif\" font-size=\"14.00\">s1 : m1, s2</text>\n",
       "</g>\n",
       "<!-- 2&#45;&gt;3 -->\n",
       "<g id=\"edge3\" class=\"edge\">\n",
       "<title>2&#45;&gt;3</title>\n",
       "<path fill=\"none\" stroke=\"black\" d=\"M102.02,-71.7C102.02,-64.41 102.02,-55.73 102.02,-47.54\"/>\n",
       "<polygon fill=\"black\" stroke=\"black\" points=\"105.52,-47.62 102.02,-37.62 98.52,-47.62 105.52,-47.62\"/>\n",
       "</g>\n",
       "</g>\n",
       "</svg>\n"
      ],
      "text/plain": [
       "<__main__.show at 0x11775c3e0>"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "show(bayesTree)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "py312",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.6"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
